#include "../../../XMLTools.h"
#include "../../../MathTools.h"

#include <boost/lexical_cast.hpp>

#include "KinematicSensorMeasurement.h"

using namespace MMM;

KinematicSensorMeasurement::KinematicSensorMeasurement(float timestep, const Eigen::VectorXf &jointAngles, SensorMeasurementType type) :
    InterpolatableSensorMeasurement(timestep, type),
    jointAngles(jointAngles)
{
}

SensorMeasurementPtr KinematicSensorMeasurement::clone() {
    return clone(timestep);
}

bool KinematicSensorMeasurement::equals(SensorMeasurementPtr sensorMeasurement) {
    KinematicSensorMeasurementPtr ptr = boost::dynamic_pointer_cast<KinematicSensorMeasurement>(sensorMeasurement);
    if (ptr) {
        return jointAngles == ptr->jointAngles;
    }
    return false;
}

KinematicSensorMeasurementPtr KinematicSensorMeasurement::clone(float newTimestep) {
    KinematicSensorMeasurementPtr clonedSensorMeasurement(new KinematicSensorMeasurement(newTimestep, jointAngles, type));
    return clonedSensorMeasurement;
}

void KinematicSensorMeasurement::appendMeasurementDataXML(RapidXMLWriterNodePtr measurementNode) {
    measurementNode->append_node("JointPosition")->append_data_node(XML::toString(jointAngles));
}

Eigen::VectorXf KinematicSensorMeasurement::getJointAngles() {
	return jointAngles;
}

KinematicSensorMeasurementPtr KinematicSensorMeasurement::interpolate(KinematicSensorMeasurementPtr other, float timestep, SensorMeasurementType type) {
    int vSize = boost::lexical_cast<int>(jointAngles.rows());
    Eigen::VectorXf interpolatedJointAngles(vSize);
    for (int i = 0; i < vSize; i++) {
        //if (i < 10) std::cout << this->timestep << "  " << timestep << "  " << other->timestep << std::endl;
        interpolatedJointAngles(i) = Math::linearInterpolation(jointAngles(i), this->timestep, other->jointAngles(i), other->timestep, timestep);
    }

    KinematicSensorMeasurementPtr interpolatedSensorMeasurement(new KinematicSensorMeasurement(timestep, interpolatedJointAngles, type));
    return interpolatedSensorMeasurement;
}
